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ABSTRACT 

This  paper  focuses  on  the  latest  technology  trends  for  navigating  in  difficult  urban,  indoor,  and  underground 
environments  where  typical  Global  Positioning  System  (GPS)  receivers  do  not  function.  The  latest  alternative 
navigation  (Alt-Nav)  technologies  based  on  electro-optical  techniques  are  described.  These  techniques  extract 
features  from  electro-optical  images  (for  example,  a  point  feature  associated  with  a  corner  of  a  building  or  a 
line  associated  with  a  wall  of  an  indoor  hallway)  and  then  utilize  those  features  as  navigation  related 
landmarks  to  enable  aiding  of  inertial  navigation  system  (INS).  The  Alt-Nav  technologies  presented  include 
optically-aided  INS  and  Ladar-aided  INS.  Tightly  integrating  these  technologies  with  an  INS  should  lead  to 
navigation  performance  similar  to  that  is  achieved  in  today ’s  GPS/INS  integrations.  An  Alt-Nav  integration 
vision  for  the  future  is  given  with  some  example  configurations  that  improve  overall  navigation  system 
robustness. 
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1.0  MOTIVATION  AND  BACKGROUND 

Over  the  past  couple  of  decades,  there  have  been  a  number  of  navigation  trends  that  have  driven  the  desire  to 
improve  our  ability  to  navigate  in  all  environments.  Table  1  notionally  represents  these  trends.  Previously,  the 
primary  desire  was  to  navigate  single,  stand-alone  systems  (such  as  a  car),  but  now,  the  desire  is  increasingly 
to  have  simultaneous  navigation  awareness  of  multiple  interdependent  systems  (such  as  a  traffic  notification 
system  in  a  car).  Previously,  navigation  capability  was  not  always  counted  on,  but  increasingly  navigation  is 
considered  to  be  an  assumed  infrastructure  (like  knowing  the  lights  will  come  on  when  you  turn  on  the  light 
switch).  Previously,  navigation  accuracy  of  5-10  m  seemed  almost  extravagant  when  other  worldwide 
navigation  options  prior  to  GPS  (namely,  Omega  [1]  and  stand-alone  inertial)  had  accuracies  more  on  the 
order  of  1-2  km.  Now,  there  are  many  applications  that  require  meter  or  sub-meter  level  accuracy  (such  as 
precision  agriculture).  Previously,  due  to  cost,  power,  and  size  constraints,  it  was  generally  only  feasible  to 
know  where  the  “big  things”  are  (such  as  airplanes).  Now,  navigation  is  desired  on  more  and  more,  smaller 
and  smaller  objects  (such  as  cell  phones). 


Table  1.  Navigation  Trends 


Then 

— *•  Now 

Single,  stand-alone  systems 

Multiple  interdependent  systems 
—>  work  together  to  achieve  goal 
(requires  navigation) 

Precise  navigation  as  a  “nice-to- 
have”  entity 

Complete  dependence  on  reliable 
—>  navigation  (navigation  as  an 
assumed  infrastmcture) 

Navigation  accuracy: 

5-10  m  is  sufficient 

Sub-meter  to  cm-level  accuracy 
desired  (“Accuracy  is  addictive”) 

We  want  to  know  where  the  “big 
things”  are 

We  want  to  know  where 
everything  is 

While  GPS  has  been  the  driving  factor  behind  most  of  these  trends,  there  are  limitations  to  GPS  that  have 
become  more  evident  over  time  as  we  have  increasingly  come  to  rely  on  navigation.  The  shortfalls  in  GPS 
could  be  called  the  “navigation  gap”,  as  depicted  notionally  in  Figure  1.  The  horizontal  axis  in  this  figure 
represents  the  continuum  between  urban/indoor  and  rural/open  environments.  The  vertical  axis  roughly 
represents  altitude,  from  ground  level  all  the  way  up  to  space. 

GPS  does  a  great  job  of  covering  much  of  this  two-dimensional  trade  space  (indicated  by  the  solid  blue 
shape),  but  GPS  by  itself  is  not  sufficient  when  moving  close  to  the  bottom  left  comer  of  the  graph.  Recent 
advancements  in  high-sensitivity  GPS  have  helped  to  decrease  the  size  of  this  gap  (indicated  by  the  striped 
blue  shape),  but  there  still  remains  a  gap  where  availability,  accuracy,  or  reliability  of  GPS  by  itself  is  not 
sufficient  for  many  applications.  Ironically,  it  is  in  just  such  urban/indoor  locations  where  many  people  spend 
most  of  their  time.  (In  fact,  odds  are  that  you  would  have  a  hard  time  obtaining  a  high  accuracy  GPS  fix 
wherever  you  are  reading  this  paper!) 
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Figure  1:  The  Navigation  Gap 


Summarizing  the  navigation  requirements  in  a  manner  similar  to  [2],  it  would  be  desirable  to  develop  a 
navigation  system  that  (a)  supports  an  indefinite  mission  duration,  (b)  supports  real-time  3D  location 
performance,  (c)  supports  localization  in  urban  environments  and  inside  residential  and  most  commercial 
buildings,  (d)  supports  operation  in  an  unknown  (unmapped)  or  sparsely  known  (partially  mapped) 
environment,  (e)  supports  localization  from  the  power-off  condition  and  requires  no  separate  starting  location 
initialization  of  the  user  equipment,  (f)  supports  individual  isolated  user  terminals,  (g)  shall  be  able  to  re¬ 
acquire  the  navigation  capability  after  a  temporary  loss,  (h)  is  low-cost  and  low-weight,  (i)  does  not  require 
user  motion  to  work,  (j)  shall  have  a  level  of  integrity  (assurance),  accuracy,  availability  and  continuity  of 
service  consistent  with  the  tactical  mission  requirements. 

Alternative  Navigation  Techniques 

For  the  reasons  described  above,  alternative  navigation  techniques  have  been  and  are  currently  being 
developed  to  help  fill  this  navigation  gap.  At  least  three  broad  categories  of  alternative  navigation  techniques 
exist: 

1.  Image/ladar/Doppler/DR  aiding  of  inertial.  These  techniques  attempt  to  use  an  inertial  system,  but 
constrain  the  drift  by  incorporating  another  source  or  sources  of  aiding.  Such  systems  are  typically  self- 
contained.  Examples  include  image-aided  inertial  navigation  [3],  ladar-aided  inertial  navigation  [5],  and 
pedometry -based  DR-aiding  of  inertial  [6].  This  area  of  alternative  navigation  will  be  the  focus  of  this 
paper. 

2.  Beacon-based  navigation  (including  pseudolites).  If  the  GPS  signal  is  not  adequate  for  navigation  in  a 
particular  environment,  it  is  possible  to  transmit  an  additional  signal  or  signals  that  are  specifically 
designed  for  navigation  purposes.  If  the  transmitted  signals  are  similar  to  GPS  signals,  then  such  beacon 
transmitters  are  usually  called  “pseudolites.”  Examples  of  beacon-based  navigation  systems  for  indoor 
navigation  can  be  found  in  [7]  and  [8]. 

3.  Navigation  using  signals  of  opportunity  (SoOP).  Signals  of  opportunity,  as  defined  in  this  paper,  are 
radio  frequency  (RF)  signals  that  are  not  intended  for  navigation.  Examples  from  previous  research 
include  digital  television  [9],  analog  television  [10],  and  AM  radio  [11][12].  Part  2  of  this  paper  series 
focused  on  these  techniques  [13]. 
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This  paper  is  focused  on  the  first  category  listed  above — navigation  techniques  that  use  Electro-optical 
sensors  such  as  vision  cameras  and  Ladar  (laser  radar  or  laser  detection  and  ranging)  sensors  to  aid  an  Inertial 
Measurement  Unit  (IMU).  The  basic  idea  behind  these  techniques  is  to  extract  features  from  electro-optical 
images  (for  example,  a  point  feature  associated  with  a  comer  of  a  building)  and  apply  those  features  for 
landmark-based  inertial  aiding.  Even  though  we  focus  on  vision-aided  and  Ladar-aided  navigation  in  this 
paper,  it  does  not  imply  that  the  other  alternative  navigation  approaches  are  inferior.  There  are  strengths  and 
weaknesses  to  each  approach,  and  selecting  the  appropriate  approach  requires  knowledge  of  the  constraints 
and  requirements  of  a  specific  application. 


2.0  IMAGE-AIDED  INS 

Image-aided  navigation  serves  as  an  excellent  example  of  passive  signal-of-opportunity  navigation  for  the 
following  reasons.  First,  there  is  a  strong  natural  precedent  in  the  animal  kingdom.  Many  animals  have  been 
shown  to  utilize  visual  information  for  navigation.  In  fact,  the  ocular-vestibular  system  provides  the  primary 
navigation  suite  for  humans.  Secondly,  optical  sensors  are  inherently  high-bandwidth.  This  results  in  the 
potential  for  very  precise  angular  resolution.  Finally,  digital  imaging  sensors  are  readily  available  and  easy  to 
interface  with,  which  makes  them  a  very  practical  solution  for  investigation  of  navigation  potential.  Parts  of 
this  section  are  based  on  work  presented  in  more  detail  in  [3]  and  [4]. 

Image-aiding  methods  are  typically  classified  as  either  feature-based  or  optic  flow-based,  depending  on  how 
the  image  correspondence  problem  is  addressed.  Feature-based  methods  determine  correspondence  of  features 
(or  “landmarks”)  in  the  scene  over  multiple  frames,  while  optic  flow-based  methods  typically  determine 
correspondence  for  a  whole  portion  of  the  image  between  frames.  Optic  flow  methods  have  been  proposed 
generally  for  elementary  motion  detection,  focusing  on  determining  relative  velocity,  angular  rates,  or 
obstacle  avoidance  [14].  Feature  tracking-based  navigation  methods  have  been  proposed  both  for  fixed-mount 
imaging  sensors  or  gimbal-mounted  detectors  which  “stare”  at  the  target  of  interest,  in  a  manner  similar  to  the 
gimballed  infrared  seeker  on  heat-seeking,  air-to-air  missiles.  Many  feature  tracking-based  navigation 
methods  exploit  knowledge  (either  a  priori ,  through  binocular  stereopsis,  or  by  exploiting  terrain 
homography)  of  the  target  location  and  solve  the  inverse  trajectory  projection  problem  [15]  [16].  If  no  a  priori 
knowledge  of  the  scene  is  provided,  estimation  of  the  navigation  state  is  completely  correlated  with  estimating 
the  scene.  This  is  referred  to  as  the  structure  from  motion  (SFM)  problem.  A  theoretical  development  of  the 
geometry  of  fixed-target  tracking,  with  no  a  priori  knowledge  is  provided  in  [17].  An  online  (extended 
Kalman  filter-based)  method  for  calculating  a  trajectory  by  tracking  features  at  an  unknown  location  on 
Earth’s  surface,  provided  the  topography  is  known,  is  given  in  [18]. 

2.1  Basic  Concept 

The  basic  concept  of  the  feature-based  vision/inertial  integrated  navigation  is  illustrated  in  Figure  2. 
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Figure  2:  Overview  of  the  image-aided  inertial  algorithm  [4] 


The  algorithm  consists  of  the  following  fundamental  steps:  At  time  epoch  tt  the  system  captures  an  image  of 
the  environment  using  its  digital  imaging  device  and  converts  this  image  to  a  set  of  discrete  features  expressed 
in  a  space  referred  to  as  the  feature  space.  Next,  both  the  navigation  state  and  the  feature  space  state  are 
propagated  to  time  epoch  ti+h  the  next  imaging  event.  At  ti+1  another  image  is  captured  and  transformed  to 
feature  space.  Both  the  feature  space  of  the  captured  image  frame  at  time  ti+1  and  the  propagated  feature  space 
are  input  to  the  statistical  feature  correspondence  algorithm  that  associates  features  at  time  tt  to  features  at  time 
ti+1.  Finally,  the  trajectory  error  is  estimated  using  these  associated  features  in  a  Kalman  estimator.  The  next 
couple  of  paragraphs  will  explain  these  steps  in  more  detail. 

A  digital  imaging  device  (the  vision  camera)  measures  the  light  intensity  pattern  projected  through  optics  onto 
a  sensor.  The  projection  is  a  nonlinear  function  of  the  scene  and  lighting  conditions,  the  camera  optics,  and  the 
pose  of  the  camera  relative  to  the  scene.  The  measured  image  can  be  thought  of  as  a  surface  in  three 
dimensions  (two  spatial  and  one  intensity),  which  is  corrupted  by  measurement  noise,  optical  distortions,  and 
possibly  spatial  aliasing.  While  humans  easily  interpret  camera  images,  an  automated  navigation  algorithm 
would  require  an  automated  image  pre-processing  step  that  consists  of  the  extraction  of  distinct  features  (or 
landmarks  in  terrain  referenced  navigation  (TRN)  applications)  in  the  image.  Since  the  features  are 
represented  in  their  own  space,  the  so-called  “feature  space”,  the  process  is  often  referred  to  as  the 
transformation  of  the  image  frame  to  feature  space.  In  summary,  the  feature  space  transformation  converts  the 
image  array  into  a  collection  of  feature  vectors,  located  in  the  feature  space. 

Many  potential  feature  transformation  methods  have  been  proposed.  The  most  desirable  feature  space 
transformation  for  image-aided  navigation  decomposes  the  feature  space  into  separable  pose  and  object 
dimensions.  An  ideal  set  of  pose  dimensions  would  be  completely  dependent  on  the  relative  pose  of  the 
object,  and  independent  on  the  type  of  object  or  feature.  Conversely,  the  ideal  set  of  object  dimensions  would 
be  completely  independent  of  the  relative  pose  of  the  object,  and  only  depend  on  the  type  of  object.  This  is 
similar  to  human  interpretation  of  objects.  For  example,  to  our  eyes  a  pencil  always  looks  like  a  pencil, 
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regardless  its  orientation  or  size.  Although  no  algorithm  is  currently  known  which  can  achieve  true 
independence  between  the  pose  and  object  bases,  feature  transformation  algorithms  have  been  proposed  which 
display  scale  and  rotation  independence.  An  example  of  such  a  transformation  is  based  on  the  scale-invariant 
feature-tracking  (SIFT)  algorithm  developed  by  Lowe  [19]  [4]. 

Once  a  collection  of  features  is  identified  for  the  current  image  at  time  th  the  estimated  changes  in  the 
navigation  state  are  used  to  predict  the  location  of  features  at  time  ti+1  through  a  stochastic  projection 
transformation.  This  transformation  operates  on  the  pose  dimensions  of  each  feature  vector  from  the  previous 
image  and  uses  IMU  measurements  in  a  Kalman  filter  to  predict  pose  and  pose  uncertainty  at  the  time  of  the 
next  image.  The  concept  is  illustrated  in  Figure  3. 


INERTIAL  MEASUREMENTS 
(KF  NAVIGATION  STATE  ESTIMATE) 


Figure  3:  Stochastic  feature  projection.  Optical  features  of  interest  are  mapped  into 
future  images  using  inertial  measurements  and  stochastic  projections  [3] 


This  predicted  set  of  feature  vectors  is  then  compared  to  the  features  detected  in  the  next  image  (ti+1).  This 
comparison  or  matching  procedure  compares  the  respective  feature  vectors  using  a  rigorous,  statistical 
weighting.  This  rigorous  statistical  feature  matching  method  is  based  on  inertial  measurements  and  integrates 
the  image  and  inertial  sensors  at  a  deeper  level  than  previous  methods.  Once  feature  matches  are  determined, 
the  errors  between  the  predicted  feature  location  and  the  actual  feature  location  are  used  to  correct  errors  in 
the  navigation  state.  This  is  accomplished  through  the  use  of  a  Kalman  filter.  The  Kalman  filter  mechanization 
will  be  described  in  more  detail  in  Section  2.2.  The  cycle  is  then  repeated  for  each  image  captured.  In  the  next 
section,  a  detailed  discussion  of  the  components  of  the  image  and  inertial  fusion  algorithm  is  presented. 

2.2  Mechanization 

Fusion  of  image  and  inertial  data  is  accomplished  through  the  implementation  of  an  Extended  Kalman  Filter 
(EKF)  [20] [21].  The  EKF  estimates  the  errors  in  the  calculated  system  parameters.  In  order  to  minimize  the 
effects  of  linearization  errors,  the  system  parameters  are  periodically  corrected  by  removing  the  current  error 
estimate.  A  block  diagram  of  the  image-aided  navigator  is  shown  in  Figure  4.  To  enable  derivation  of  the  EKF 
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a  relationship  between  the  basic  measurement  (the  pixel  location  of  the  feature  in  the  image  plane)  and  the 
state  (position  and  velocity  of  the  user)  must  be  established.  This  relationship  is  non-linear  and  a  general 
expression  is  given  by: 


z(t,)  =  h{pN(t1),C^(t1),yN(t1),Tf  }+  v(t,)  (1) 

In  (1),  pN  is  the  user  position  in  the  navigation  frame,  Cb  is  the  body  to  navigation  frame  direction  cosine 
matrix  (DCM),  yN  is  the  feature  location  in  the  navigation  frame,  Tcpix  is  the  homogeneous  camera  projection 
matrix,  v  is  additive  white  Gaussian  noise  (AWGN),  and  h  is  a  non-linear  function. 


Figure  4:  Image-aided  inertial  navigation  filter  block  diagram  with  EKF 


Similar  to  equation  (1),  a  non-linear  relationship  exists  between  the  feature  location  in  the  navigation  frame 
and  the  feature  location  measurements  output  by  the  feature  extraction: 

yN(ti) =g{pN(ti),c^(ti),z(ti),d(ti),c^x<fix }  (2) 

where  d  is  the  distance  form  the  camera  to  the  landmark  and  Chc  is  the  camera  to  body  DCM.  In  equations  (1) 
and  (2)  the  tilde  indicates  that  these  quantities  include  an  error  component. 

In  the  EKF  implementation  described  in  [4],  the  navigation  error  state  vector,  8x,  consists  of  the  following 
fifteen  elements: 

8x  =  [5Pn  Sv£  v|/t  (3) 

where  8Pn  is  the  position  error  state  expressed  in  the  navigation  frame  (3x1  vector),  SvN  is  the  velocity  error 
state  expressed  in  the  navigation  frame  (3x1  vector),  vj/  is  the  attitude  (3x1  vector),  a b  is  the  accelerometer 
bias  expressed  in  the  body  frame  (3x1  vector),  and  bb  is  the  gyroscope  bias  expressed  in  the  body  frame  (3x1 
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vector).  Feature  locations  are  incorporated  in  the  Kalman  filter  by  augmenting  the  state  vector  in  equation  (3) 
with  a  vector  containing  the  feature  (landmark)  errors: 

8y=[5ySj  -  5yN,M]T  (4) 

Features 


This  addition  enables  tracking  of  these  features  over  time  and  using  the  features  for  IMU  error  calibration. 
The  linearized  measurement  model  required  for  the  EKF  implementation  can  now  be  derived  from  equation 
(1)  by  linearizing  the  non-linear  function  h  around  the  nominal  trajectory  by  finding  the  derivatives  of  h  with 
respect  to  the  state  vector  elements  x  and  feature  elements  y,  and  evaluating  these  at  the  nominal  trajectories 
x, and  y : 


5zz,i(ti+i)  =  Hm8x(ti+1)  +  Hzyn .  Syni(ti+1)  +  v(tj)  (5) 

where  Hzx  and  Hzy  are  referred  to  as  the  influence  or  observation  matrices. 

Incorporating  landmarks  or  features  of  opportunity  into  the  navigation  filter  requires  two  steps:  1)  determining 
the  initial  feature  location  and  associated  influence  matrices  (H^),  and  2)  calculating  the  measurement  errors 
and  associated  influence  matrices  (Hzy).  Various  methods  exist  to  determine  the  initial  feature  location  and 
associated  influence  matrices.  The  methods  differ  in  the  manner  in  which  they  estimate  the  distance,  d,  from 
the  camera(s)  to  the  feature  used  in  equation  (2).  Reference  [4]  addresses  three  methods  in  detail:  a) 
feature/landmark  location  estimation  using  a  statistical  terrain  model  (one  camera  and  a  map),  (b)  a  feature 
location  estimation  using  binocular  stereopsis  (two  cameras),  (c)  and  a  feature  location  estimation  using 
egomotion  and  monocular  measurements  (one  camera  over  time). 

To  complete  the  EKF,  a  state  equation  is  required  as  well.  This  state  equation  or  system  model  is 
characterized  by  state  transition  matrix  F.  The  EKF  implementation  is  a  discrete-time  implementation  of  the 
continuous-time  first  order  Gauss-Markov  state  space  equation: 

Si(t)  =  F(t)Sx(t)  +  G(t)w(t)  (6) 

The  state-transition  matrix  F  and  its  entries  can  be  found  in  [4],  and  for  the  derivation  of  its  discrete-time 
version,  O,  the  reader  is  referred  to  references  [4]  and  [20]. 

In  the  EKF  implementation,  the  state-transition  matrix  O  is  used  to  project  both  the  navigation  error  state  (Sx) 
and  the  feature  error  state  (Sy)  and  the  covariances  of  these  error  states  from  one  time  epoch  to  the  next 
epoch,  resulting  in  an  navigation  and  feature  error  prediction.  Next,  the  observations,  the  influence  matrix,  and 
the  prediction  covariances  are  used  to  calculate  the  Kalman  gain  and  update  the  prediction  to  obtain  the  error 
estimate  for  time  epoch  ti+1.  This  iterative  process  of  projecting  and  updating  the  state  vector  is  illustrated  in 
Figure  5.8  of  [21]. 

2.3  Performance  Demonstration 

This  section  will  show  some  test  results  of  the  algorithms  discussed  in  the  previous  section  including 
simulation  results  and  results  from  actual  operation  in  a  real  world  environment.  The  environment  in  both 
scenarios  was  an  indoor  hallway  environment.  The  simulated  hallway  environment  was  simulated  to  be 
straight  and  3m-by-3m-by-100m  in  size  with  randomly  spaced  features  on  walls,  floors,  and  ceilings  (0.25 
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features  /  m2).  The  motion  profile  consisted  of  a  10-minute  straight  trajectory.  Figure  5  shows  the  results  for 
the  horizontal  error. 


Figure  5:  Simulated  60-run  Monte  Carlo  root-sum-squared  (RSS)  horizontal  position  error 
for  indoor  profile  using  both  consumer-grade  and  tactical-grade  inertial  sensors 


Figure  6  shows  the  results  for  some  real  indoor  test  data  using  both  tactical  and  navigation-grade  sensors.  The 
profile  consisted  of  a  closed  path  in  an  indoor  environment.  The  path  began  and  ended  at  the  same  location 
and  orientation  in  the  Advanced  Navigation  Technology  (ANT)  Center  laboratory,  at  the  Air  Force  Institute  of 
Technology.  As  in  the  previous  profile,  the  data  collection  began  with  a  10  min  stationary  alignment  period, 
followed  by  a  10  min  loop  around  the  hallways  of  the  building.  The  sensor  was  pointed  primarily  in  the 
direction  of  travel.  No  prior  knowledge  was  provided  to  the  algorithm  regarding  the  location  of  features  or 
structure  of  the  environment.  A  sample  image  from  the  indoor  profile  is  shown  in  Figure  7.  The  indoor  profile 
presents  the  algorithm  with  various  challenges  from  a  feature  tracking  perspective.  The  repetitive,  visually 
identical  features  (e.g.,  floor  tiles,  lights,  etc.)  present  in  the  indoor  environment  at  times  confuse  the  feature¬ 
tracking  algorithm.  In  addition,  reflections  from  windows  and  other  shiny  surfaces  are  not  always  properly 
interpreted  the  filter  resulting  in  the  occasional  navigation  errors.  Finally,  the  lower  light  intensity  levels  and 
large  areas  with  poor  contrast  (e.g.,  smooth,  featureless  walls)  present  a  relatively  stark  feature  space. 
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Figure  6:  Results  of  the  image-aided  inertial  navigator  using  tactical  and  navigation  grade  IMUs. 


Figure  7:  Example  of  the  feature-tracking  algorithm  in  an  indoor  environment 
2.4  Multi-Aperture  Vision  Aiding 

This  section  discusses  extension  of  the  vision-aided  inertial  navigation  approach  for  multi-aperture  camera 
cases.  Inspired  from  biological  systems,  a  multi-aperture  vision  processing  system  allows  for  accurate  motion 
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estimation  by  observing  optical  flow  across  all  apertures.  The  multi-aperture  approach  is  particularly  well 
suited  for  resolving  the  motion  scale-factor  ambiguity  (i.e.,  for  the  estimation  of  ranges  to  vision-based 
features)  by  providing  a  wide  filed  of  regard  for  detecting  and  tracking  visual  features.  Figure  8  illustrates  an 
example  implementation  of  the  multi-aperture  camera  system. 


Figure  8:  Multi-aperture  experimental  setup  developed  by  the  Alt-Nav  team 
of  the  Air  Force  Research  Laboratory’s  Munitions  Directorate;  the  setup  includes 
three  video  cameras  with  a  90-deg  separation  of  their  optical  axes 

A  unified  coordinate  frame  has  to  be  chosen  for  multi-aperture  image  processing.  Particularly,  features 
observed  by  different  cameras  must  be  converted  into  a  single  frame  of  reference.  Each  camera  originally 
represents  its  pixels  by  their  two-dimensional  (2D)  Cartesian  coordinates  resolved  in  the  camera  focal  plane 
(these  coordinates  are  generally  referred  to  as  homogeneous  coordinates).  In  principle,  a  Cartesian 
representation  can  be  also  adapted  for  the  unified  frame,  which,  in  this  case,  will  be  represented  by  a  planar 
surface.  However,  this  representation  has  a  drawback  of  singularity  cases  as  shown  in  Figure  9. 


Unified  plane 


Feature  unit 


No  projection  exists  for  features 
with  unit  vectors  e  collinear  to  the 
unified  planar  surface. 


For  features  whose  unit  vectors  are  almost 
collinear  to  the  unified  plane,  small  feature 
errors  result  in  large  projection  errors. 


Figure  9:  Drawbacks  of  the  Cartesian  representation  of  the  unified 
coordinate  frame  for  the  multi-aperture  camera  case 
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Homogeneous  feature  coordinates  extracted  from  an  image  of  a  particular  camera  must  be  projected  onto  a 
planar  surface  that  is  chosen  for  the  multi-aperture  frame.  For  those  features  whose  unit  vector  (i.e.,  a  unit 
vector  pointed  from  the  camera  focal  point  to  the  feature)  is  collinear  to  the  unified  plane,  such  a  projection 
does  not  exist.  If  the  unit  vector  is  almost  collinear  to  the  unified  plane,  then  small  errors  in  feature 
measurements  result  in  large  projection  errors. 

To  remove  singular  cases  associated  with  the  Cartesian  representation,  a  unit  sphere  approach  is  chosen  for 
multi-aperture  cameras.  This  approach  is  motivated  by  nature’s  multi-aperture  vision  systems,  such  as  the 
compound  eyes  of  insects,  which  process  optical  flow  by  projecting  it  onto  a  sphere.  The  unit  sphere  provides 
a  natural  framework  for  wide  field-of-view  (>180  degree)  sensors,  where  the  traditional  focal  plane 
representation  is  not  possible.  Figure  10  shows  the  unit  sphere  feature  projection. 


Figure  10:  Unit  sphere  frame  and  unit  sphere  feature  projection 


Features  that  are  extracted  from  multiple  cameras  are  projected  on  the  surface  of  a  unit  sphere  and  represented 
by  their  spherical  azimuth  and  elevation  angles.  In  Figure  10,  the  unit  sphere  origin  is  collocated  with  the 
camera’s  focal  point.  However,  this  is  not  required,  and  the  unit  sphere  projection  can  be  generalized  for  cases 
where  cameras’  focal  points  are  not  collocated  with  the  sphere  origin  and  with  each  other.  Figure  1 1  illustrates 
the  feature  projection  for  the  non-collocated  case. 
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Figure  11 :  Unit  sphere  feature  projection  for  the  case  of  non-col  located  cameras 

Equation  (7)  defines  unit  sphere  projections: 


Rf  =  Cr-[mx  my  F]T-Rc 

ef  =  Rf/|Rf||  (7) 

cp  =  arctan(efy ,efx)  cp  =  arcsin(efz) 

where  C™  is  the  coordinate  transformation  matrix  from  the  camera  {XC,YC,ZC} -frame  into  the  {XS,YS,ZS}- 
frarne  attached  to  the  unit  sphere;  mx,my  are  homogeneous  feature  coordinates  extracted  from  the  image,  F  is 
the  camera  focal  length,  Rc  is  the  vector  that  originates  from  the  unit  sphere  origin  and  ends  at  the  camera 
focal  point,  and  arctanQ  is  the  four-quadrant  arctangent  function.  The  main  advantage  of  the  unit  sphere 
formulation  is  the  removal  of  singularity  projection  cases  that  are  present  if  Cartesian  representation  is  utilized 
(see  Figure  9  above). 

The  use  of  the  unit  sphere  feature  projections  requires  the  transformation  of  motion  equations  (i.e., 
relationships  between  feature  parameters  and  motion  parameters)  from  the  Cartesian  frame  into  the  spherical 
frame.  This  transformation  is  illustrated  below  for  a  2D  motion  case  where  only  a  translation  motion 
component  is  present.  In  this  case,  the  unified  frame  is  represented  by  a  unit  circle  as  shown  in  Figure  12. 
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Feature  at  image  1 


Figure  12:  Unit  circie  representation  for  a  2D,  translation  only  motion  case 


From  the  geometry  shown  in  Figure  12:  a/cos Acp  =  b/sinAcp ;  where  a  =  p(1)-^AR,e(1)^;  b  =  -(AR,e(11)^; 


Acp  =  cp(1)  -cp(Zj;  (,)  is  the  vector  dot  product;  p 


„(2) 


(1)-  R(1) 


is  the  distance  to  the  feature  for  image  1;  e(1)  is  the 


feature  unit  vector  for  image  1;  and,  is  the  unit  vector  perpendicular  to  e1  \  Correspondingly,  the 
following  constraint  equation  is  formulated: 


{p(1)  -  (AR,  e(1)  ^  cos  Acp  =  -(AR,  e(jJ}  ^  cos  Acp 
This  equation  can  be  rearranged  as  follows: 

p(1)  •  cos  Acp  =  -(AR,  e(j_2)  J 


(8) 

(9) 


Note  that  Acp  is  directly  calculated  from  camera  measurements.  Also,  equation  (9)  only  includes  the  unknown 
range  to  the  feature  at  the  first  image.  The  knowledge  of  range  information  for  other  images  (second,  third, 
etc.)  is  not  required.  Once  the  initial  range  is  estimated,  the  motion  constraint  (9)  can  be  applied  to  formulate 
Kalman  filter  vision/inertial  measurement  observables  for  other  images  without  the  need  to  know  (or  measure) 
their  feature  ranges.  As  compared  to  a  complete  motion  formulation  that  includes  feature  range  variables  for 
the  current  image  (see  equation  (2)  above),  the  motion  constraint  formulation  is  beneficial  for  a  monocular 
camera  case,  as  well  as  for  stereo  cases  with  a  limited  baseline,  where  direct  measurements  of  the  range  value 
are  not  possible  or  inefficient. 

For  a  general  3D  case  that  includes  translational  and  rotational  motion  components,  two  motion  constraint 
equations  are  derived  in  a  manner  similar  to  the  2D  consideration  above.  The  first  constraint  is  obtained  by 
considering  the  horizontal  component  of  the  translational  motion,  and  the  second  constraint  is  formulated  by 
adding  the  vertical  motion  to  it.  Constraint  equations  are  then  modified  to  incorporate  the  rotational  motion 
component.  The  final  constraint  equations  are  as  follows: 
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(e(2))T  •  AC^j  •  B  •  AR  =  (e(1))T  •  BT  •  AC*  •  e(2)  •  p(1) 

(e(2))T-AC^Dp-AR=(e(2))T-AC^e±(1)-p(1)  (  } 

where  AR  and  AC *  are  position  and  orientation  changes  between  images  1  and  2;  and,  matrices  B  and  D  are 
defined  as: 


’o  -i  o’ 

0 

0 

-cos(cp(1>) 

B  = 

1  0  0 

,D  = 

0 

0 

-sin(cp(1)) 

0  0  0 

cos(cp(1)) 

sin((p(1)) 

0 

Motion  constraint  equations  (10)  are  applied  to  formulate  Kalman  filter  observables  r|palman  for  the  multi¬ 
aperture  vision  inertial  aiding: 


T| 


Kalman 

P 


(e<„»)T  ■  4C‘  •  B  ■  ARms  -  (i">j  ■  BT  •  ACf  •  e<p2»  •  p® 
(e<»)T  ■  ACi  •  Dp  •  ARms  -  (e>»j  ■  ACj  •  8®  •  p® 


,P 


(12) 


where  eps),  p  =  l,..,P,  s  =  l,2  is  the  unit  vector  of  the  feature  p  that  is  extracted  from  image  s;  pp1}  is  the 

estimated  feature  range  for  image  1,  and,  AR^g  and  AC^  are  INS  position  and  orientation  changes  between 
images.  Image  1  is  generally  the  image  where  the  feature  was  first  observed  and  image  2  is  the  current  image. 

The  initial  feature  range  p(1)  is  estimated  by  observing  the  feature  from  two  different  locations  of  the  platform 

and  using  inertial  displacement  and  orientation  change  measurements.  This  estimation  is  derived  from 
Equation  (12)  and  is  formulated  as  follows: 


(®p2))T  ‘  Cn  •  B  •  AR.jj.jg 

(fp  *)  ’  '  Dp  ‘ 


(f p*)1  ’ BT  '  AC^1  •  Sp 
(e^AC^ 


(13) 


Essentially,  the  image  depth  is  initialized  using  synthetic  stereo-vision:  camera  motion  is  applied  to  synthesize 
a  stereo-vision  baseline,  which  is  measured  by  the  INS.  Note  that  in  this  case  a  correlation  between  depth 
estimation  errors  and  inertial  position  and  orientation  errors  is  introduced.  This  correlation  must  be  taken  into 
account  by  the  Kalman  filter  design.  Particularly,  the  filter  states  include  INS  error  states,  and  errors  in  initial 
range  estimates.  Hence,  the  correlation  between  INS  position  and  orientation  errors  and  range  estimation 
errors  must  be  incorporated  into  the  state  covariance  matrix  of  the  Kalman  filter.  An  approach  for  computing 
the  INS/vision  correlation  for  the  multi-aperture  case  is  similar  to  the  computation  of  INS/vision  correlation 
for  a  single  aperture  case,  which  is  described  in  [4]. 
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Performance  of  multi-aperture/vision  inertial  aiding  was  assessed  in  a  simulated  indoor  environment.  Figure 
13  shows  a  2D  projection  of  the  simulation  environment.  The  simulation  scenario  included  motion  in  an 
indoor  hallway.  Hallway  walls  were  simulated  as  vertical  with  a  2.5-m  height.  For  each  wall,  four  point 
features  were  uniformly  distributed  over  its  length  and  height.  The  simulated  motion  trajectory  was  two- 
dimensional.  However,  the  vision/inertial  system  estimated  all  six  degrees  of  motion  freedom.  Inertial 
measurement  unit  was  simulated  as  a  lower-grade  unit  with  an  accelerometer  bias  of  1  mg  and  a  gyro  drift 
stability  of  50  deg/hr.  Specifications  of  video  cameras  were  simulated  as  follows:  640x480  resolution,  10  Hz 
update  rate,  40  deg  by  30  deg  field-of-view  (FoV).  Errors  of  extracting  a  point  feature  from  camera  images 
were  simulated  as  a  camera  quantization  error  plus  a  random  Gaussian  noise  error  with  a  standard  deviation  of 
one  pixel. 


Simulated  indoor  environment 


- walls 

O  point  features 

- motion  trajectory 


-100  -50  0  50 

X,  m 


Figure  13:  Simulated  indoor  environment  implemented  for 
the  evaluation  of  multi-aperture  vision/INS  integration 

Figure  14  exemplifies  simulation  results.  Position  accuracy  performance  is  shown  for  cases  of  a  free-mnning 
inertial,  a  single  camera/INS  integration,  and  a  multi-aperture  vision/INS  integration  including  cases  of  three 
and  eight  apertures.  For  the  multi-aperture  cases,  the  angular  separation  between  individual  cameras  was 
implemented  as  120  deg  and  45  deg  for  the  three-aperture  and  eight-aperture  cases,  accordingly. 
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Free-running  INS:  position  errors,  km 


Three  apertures/INS:  position  errors,  m 


Single  aperture/INS:  position  errors,  m 


Eight  apertures/INS:  position  errors,  m 


Figure  14:  Example  simulation  results  for  the  indoor  simulation  environment 

Simulation  results  presented  demonstrate  that  for  the  simulation  scenario  implemented  the  use  of  multi¬ 
aperture  INS  aiding  allows  reducing  position  errors  by  two  orders  of  magnitude  as  compared  to  the  single¬ 
aperture  case:  position  drift  is  reduced  from  a  100-m  level  to  a  1-m  level.  The  accuracy  performance  is 
improved  due  to  the  following  factors.  Firstly,  the  number  of  available  features  generally  increases  by 
increasing  the  camera  FoV  using  multiple  apertures.  Secondly,  and  more  importantly,  the  use  of  multiple 
apertures  enhances  the  feature  range  initialization  capabilities.  Figure  15  illustrates  this  effect.  The  range 
estimation  accuracy  is  primarily  influenced  by  the  feature  angular  change  between  images  that  are  used  for  the 
estimation.  The  determinant  of  the  Hp  Hp  matrix  in  the  pseudo-inverse  formulation  of  equation  (13)  is 

directly  related  to  changes  in  spherical  angles.  An  increased  angular  change  increases  the  determinant  value 
and  thus  improves  the  estimation  accuracy.  The  maximum  angular  change  is  accumulated  when  passing  over 
a  feature  and  observing  this  feature  with  a  side-looking  camera  as  shown  in  Figure  15.  As  a  result,  this  feature 
is  efficiently  initialized  after  a  small  platform  displacement.  Kalman  filter  observables  for  this  feature  are 
then  applied  to  correct  inertial  drift  over  longer  displacement  intervals  that  are  required  in  order  to  accumulate 
an  angular  change  that  is  sufficient  to  initialize  the  feature  range  for  the  front-looking  camera. 
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Large  angular  change  is 
accumulated  for  the  feature 
observed  by  the  side-looking  camera 


Platform L 
position  1 


Platform 
position  2 


Small  angular  change  is 
accumulated  for  the  feature  observed 
by  the  forward-looking  camera 


Figure  15:  Improved  range  initialization  capabilities  using  multi-aperture  vision:  large  angular 
change  is  accumulated  over  a  small  distance  for  the  feature  observed  by  the  side-looking 
camera;  this  feature  is  initialized  and  then  applied  to  correct  inertial  drift  over  a  longer 
distance  that  is  required  to  accumulate  an  angular  change  sufficient  for 
initializing  the  feature  observed  by  the  front-looking  camera 


3.0  L AD AR- AIDED  INS 

Whereas  image-aided  navigation  uses  passive  electro-optical  sensors,  laser-based  navigation  makes  use  of 
active  sensors:  laser  scanners  (Ladars)  or  imaging  lasers.  The  use  of  Ladar  for  navigation  purposes  has  been 
demonstrated  for  both  en-route  navigation  [22]  and  precision  approach  guidance  [23]  in  a  laser-based  TRN 
and  has  been  shown  to  provide  meter-level  accuracies  [2].  However,  TRN  techniques  are  operationally  limited 
by  the  availability  of  an  on-board  terrain  database  at  the  location  of  interest.  More  recently,  a  method  has  been 
proposed  to  perform  the  navigation  function  using  two  airborne  scanning  Ladars  integrated  with  an  INS  [24]. 
Due  to  the  lack  of  terrain  in  urban  and  indoor  environments,  the  Ladar-based  methods  must  exploit  features 
such  as  surfaces,  corners,  points,  etc.  For  the  feature-based  navigation,  changes  in  position  and  orientation  are 
estimated  from  changes  in  the  parameters  of  features  that  are  extracted  from  Ladar  images.  Two-dimensional 
(2D)  laser  scanners  and  feature -based  localization  methods  have  been  used  extensively  to  enable  navigation  of 
robots  in  an  indoor  environment.  For  example,  reference  [25]  describes  a  method  to  estimate  the  translation 
and  rotation  of  a  robot  platform  from  a  set  of  extracted  lines  and  points  using  a  2D  sensor.  Reference  [26] 
discusses  the  feature  extraction  and  localization  aspects  of  mobile  robots  and  addresses  the  statistical  aspects 
of  these  methods,  whereas  reference  [27]  introduces  improved  environment-dependent  error  models  and 
establishes  relationships  between  the  position  and  heading  uncertainty  and  the  laser  observations,  thus 
enabling  a  statistical  assessment  of  the  quality  of  the  estimates.  In  [28],  2D  scanning  LADAR  measurements 
are  tightly  integrated  with  IMU  measurements  to  estimate  the  relative  position  of  a  van  in  an  urban 
environment.  The  idea  of  using  3D  measurements  and  planar  surfaces  for  2D  localization  is  introduced  in 

[29] .  Note  that  the  above  applications  focus  on  2D  navigation  (two  position  coordinates  and  a  platform 
heading  angle).  However,  for  applications  such  as  autonomous  aerial  vehicles,  a  3D  navigation  solution  is 
required,  especially,  for  those  cases  where  the  platform  attitude  varies  in  pitch,  roll,  and  yaw  directions.  To 
enable  3D  navigation,  the  utilization  of  the  laser  range  scanner  measurements  must,  somehow,  be  expanded 
for  estimation  of  3D  position  and  attitude.  The  use  of  3D  features  from  3D  imaging  cameras  was  introduced  in 

[30] .  The  discussion  in  this  paper  will  limit  itself  to  2D  and  3D  navigation  using  2D  scanning  Ladar  sensors. 
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3.1  Basic  Concept 

Figure  16  illustrates  the  basic  measurement  mechanism  that  underlies  laser  scanner-based  navigation.  The 
system  uses  a  scanning  Ladar  to  make  range  and  angular  measurements  of  its  environment  and  converts  these 
“raw”  measurements  to  a  set  of  points  in  a  two-dimensional  (2D)  Ladar  coordinate  frame  using  a  polar 
coordinate  transformation.  Features  are  extracted  from  Ladar  scans  at  different  times  and  used  to  determine  a 
change  in  position  and  orientation  of  the  user.  Data  from  scanning  Ladar  sensors  can  support  both  2D  [28] 
and,  by  enabling  gimballing  of  the  sensor,  three-dimensional  (3D)  navigation  [31]. 


Figure  16:  Laser  Scanner-Based  Navigation 

The  discussion  in  this  paper  will  focus  on  3D  navigation  since  2D  navigation  (with  line  segments)  is  a 
considered  a  special  case  of  3D  navigation.  The  features  that  are  exploited  are  “lines”  for  2D  navigation  and 
“planar  surfaces”  for  3D  navigation.  The  rationale  for  the  use  of  lines  and  planar  surfaces  for  navigation  in  3D 
urban  environments  is  that  these  features  are  common  in  man-made  environments.  To  exemplify  this,  Figure 
17  shows  images  of  typical  urban  indoor  (hallway)  and  outdoor  (urban  canyon)  environments.  Multiple  planes 
can  be  observed  in  both  images.  Since  changes  in  image  feature  parameters  between  scans  at  different  time 
epochs  are  used  for  navigation,  the  feature  must  be  observed  in  both  scans.  Feature  repeatability  is  thus 
essential  for  Ladar-based  navigation.  Planar  surfaces  satisfy  this  requirement,  as  they  are  highly  repeatable 
from  scan  to  scan.  As  long  as  a  wall  of  a  building  stays  within  the  Ladar  measurement  range  and  FoV,  the 
plane  (or  line)  association  process  is  likely  to  be  successful. 


Indoor  image  Outdoor  image 


Figure  17:  Examples  of  planar  surfaces  observed  in  urban  images:  multiple 
planes  can  be  extracted  for  indoor  and  outdoor  image  examples. 


Figure  18  shows  a  block  diagram  of  the  Ladar-based  navigation  method.  The  desired  features  (“lines”  or 
“planar  surfaces”)  are  extracted  from  the  Ladar  scans  and  used  to  estimate  the  navigation  solution  that  is 
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comprised  of  changes  in  Ladar  position  and  orientation.  In  order  to  use  a  planar  surface  for  the  estimation  of 
position  and  orientation  changes,  this  planar  surface  must  be  observed  in  both  scans  and  it  must  be  known 
with  certainty  that  the  plane  in  one  scan  corresponds  to  the  plane  in  the  next  scan.  Hence,  the  feature 
association  (or  matching)  procedure  establishes  a  correspondence  between  planes  extracted  at  the  current  scan 
time  epoch  and  the  planes  extracted  at  previous  scan  epochs.  The  navigation  routine  stores  planes  extracted 
from  previous  scans  into  the  plane  list,  which  is  initialized  with  the  planar  surfaces  of  the  first  scan.  If  a  new 
planar  surface  is  observed  during  one  of  the  following  scans,  the  plane  list  is  updated  to  include  this  new 
plane.  Reference  [28]  discuses  how  INS  data  is  used  to  match  extracted  “line”  features  for  the  2D  navigation 
case.  In  order  to  use  INS  data  for  plane  matching,  the  line-matching  algorithms  developed  in  [28]  were 
extended  to  the  3D  case.  The  feature  matching  procedure  uses  position  and  orientation  outputs  of  the  INS  to 
predict  the  planar  surface  location  and  orientation  at  the  current  scan  time  based  on  planar  surface  parameters 
observed  at  previous  scan  time  epochs.  If  the  predicted  planar  surface  parameters  and  the  parameters  of  one  of 
the  extracted  planar  surfaces  match  closely,  a  successful  association  is  declared.  Following  successful  feature 
association  changes  in  the  associated  planar  surface  parameters  are  used  to  estimate  the  navigation  and 
attitude  solution.  Furthermore,  these  changes  in  planar  surface  parameters  are  also  applied  to  periodically  re¬ 
calibrate  the  INS  to  reduce  drift  terms  in  inertial  navigation  outputs  in  order  to  improve  the  quality  of  the  INS- 
based  plane  prediction  used  by  the  feature  matching  procedure. 


Scanning 

Ladar 


Motion 

Compensation 


INS 


Change  in  Plane 
Parameters 


H 


INS  Calibration 
(Kalman  Filter) 


Navigation 
Solution  Estimator 


Changes  in 
position  and 
orientation 


Figure  18:  Generic  routine  of  3D  navigation  that  uses  images  of  a  scanning  LADAR. 


The  position  and  orientation  data  from  the  INS  is  also  used  to  compensate  for  the  motion  of  the  platform 
(Ladar)  during  the  scans  in  those  cases  where  such  motion  could  introduce  significant  distortions  to  Ladar 
scan  images.  The  above  method  results  in  the  calculation  of  a  relative  navigation  solution.  Estimating  local 
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frame  position  and  orientation  in  one  of  commonly  used  navigation  frames  (e.g.  East-North-Up  and  Earth- 
Centered-Earth-Fixed  frames)  allows  for  the  transformation  of  the  relative  navigation  solution  into  an  absolute 
navigation  solution.  Navigation  herein  is  performed  in  completely  unknown  environments.  No  map 
information  is  assumed  to  be  available  a  priori. 

3.2  Mechanization 

Planar  surface  based  navigation  is  performed  using  the  perceived  location  changes  of  the  normal  point  on  the 
planar  surface  between  scans  at  different  time  epochs.  A  planar  surface  normal  point  is  defined  as  the 
intersection  of  the  plane  and  a  line  originating  from  the  Ladar  location  perpendicular  to  the  plane  of  interest. 
Figure  19  illustrates  the  geometry  involved  in  navigating  off  the  plane  normal  points. 


orientation  at  scan  i 

Figure  19:  Use  of  plane  normal  points  for  navigation:  changes  in  perceived  location  of  the 
normal  point  between  scan  i  and  scan  j  are  applied  to  estimate  position  changes 


In  Figure  19,  AR  is  the  delta  position  vector  (displacement  vector  between  scans  i,  at  time  ti?  and  scan  j,  at 
time  tj,  in  this  case);  ni  is  the  plane  normal  vector  whose  components  are  resolved  in  the  Ladar  body  frame  at 
scan  epoch  f;  nj  is  the  plane  normal  vector  whose  components  are  resolved  in  the  Ladar  body  frame  at  scan 
epoch  tj  ;  and,  pi  and  pj  are  the  shortest  distances  from  the  Ladar  to  the  plane  at  epochs  f  and  tj  ,  respectively. 
These  distances  are  referred  to  as  the  normal  point  distances.  Note  that  in  the  navigation  frame,  the  planar 
surface  normal  vectors  at  epochs  f  and  tj  are  equal  since  stationary  planar  surfaces  are  assumed.  However, 
expressed  in  the  Ladar  body  frame  both  normal  vectors  are  likely  to  be  unequal  due  to  the  body  frame  rotation 
between  epochs  t,  and  tj.  From  the  geometry  presented  in  Figure  19,  a  relationship  can  be  derived  between  the 
projection  of  the  displacement  vector  (between  epochs  f  and  tj)  onto  the  planar  surface  normal  vector  and  the 
change  in  the  normal  point  range  between  scans  i  and  j: 
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AR'Il;  =  Pi  —  Pj 

Given  M  associated  planar  surfaces,  a  set  of  linear  equations  like  (14)  can  be  set  up  in  matrix  form: 


(14) 


where: 


H  •  AR  =  Ap 
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Note  that  a  minimum  of  three  non-collinear  planar  surfaces  is  required  for  the  observation  matrix,  H,  to  be 
non-singular  and  thus  allowing  for  a  unique  solution  of  equation  (16).  Using  Ordinary  Least  Squares  (OLS),  a 
solution  can  be  computed  from  equation  (16)  as  follows: 


AR  =  (HTH)HTAp  (17) 

In  equation  (17),  Ap  is  the  estimated  delta  range  vector  obtained  from  the  planar  surface  extraction  and 
parameter  estimation  process.  For  2D  navigation,  “lines”  are  used  and  assumed  to  be  part  of  a  vertical  planar 
surface  where  possible  tilt  of  the  vehicle  or  planar  surface  is  estimated  separately  [28].  In  3D  navigation  the 
Ladar  scanner  is  rotated  intentionally  and  scans  at  three  (or  more)  discrete  and  known  elevations  are  used  to 
estimate  the  planar  surface  parameters  [31]. 

The  attitude  of  the  Ladar  platform  is  estimated  using  the  planar  surface  normal  vectors.  Equation  (18)  relates  a 
change  in  the  Ladar  orientation  from  time  epoch  tj  to  f  ,  given  by  DCM  C-,  to  the  change  in  the  orientation  of 
the  normal  vectors: 

Cj-nj  =  ni  (18) 

To  compute  the  DCM,  the  attitude  estimation  algorithm  needs  to  solve  Wahba’s  problem  [32]:  given  a  first 
set  of  normal  vectors  with  vector  components  resolved  in  the  Ladar  frame  at  f  and  the  second  set  of  the  same 
vectors  with  their  components  resolved  in  the  Ladar  frame  at  tj,  find  the  DCM  that  brings  the  second  set  into 
best  least  square  correspondence  with  the  first.  At  least  two  non-collinear  normal  vectors  are  required  for  the 
attitude  estimation.  Attitude  is  generally  estimated  by  solving  an  eigenvalue/eigenvector  problem,  which 
requires  solution  of  non-linear  equations.  Solution  approaches  have  been  described  by  various  sources  such  as 
[33],  [34]  and  [35]. 

In  order  to  use  planar  surfaces  for  the  estimation  of  position  and  orientation  changes  as  formulated  earlier  in 
this  section,  it  must  be  known  with  certainty  that  a  plane  at  time  epoch  f  corresponds  to  the  plane  in  at  time 
epoch  tj.  To  enable  association  of  planar  features,  INS  data  can  be  used  to  predict  the  planar  surface  range  and 
normal  vector  at  time  tj  based  on  range  and  normal  vector  estimated  at  time  f: 

Pj  =Pi-(ARlNS>ni)  (19) 

h]=AC[-ni 
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where  Pj  and  are  the  predicted  range  and  normal  vector;  p{  and  n{  are  plane  range  and  normal  vector 
extracted  at  time  i;  and,  AR^  and  AC:j  are  the  INS  measurements  of  the  position  change  vector  and  DCM 
increment  from  time  epoch  f  to  time  epoch  tj  .  If  the  predicted  range  and  normal  vector  (pj  and  np  match 
closely  to  the  range  and  normal  vector  extracted  from  scan  j  ( p  ■  and  n- ),  the  planar  surface  correspondence  is 

established  between  scans  i  and  j.  Note  that  planar  surface  matching  thresholds  must  accommodate  both 
planar  surface  extraction  errors  and  INS  drift  errors. 

Ladar  data  is  also  used  for  the  in-motion  calibration  of  the  inertial  error  states  in  order  to  reduce  drift  of  the 
INS  navigation  outputs.  The  INS  calibration  part  and  Ladar-based  position  computations  are  separated.  Since 
the  INS  is  calibrated  in  the  Ladar  measurement  domain,  the  integration  mechanization  is  considered  tightly 
coupled.  Similar  to  the  case  of  GPS/INS  integration,  one  of  the  main  advantages  of  tight  coupling  over  a 
loosely  coupled  position-domain  INS  calibration  is  the  ability  to  reduce  inertial  drift  for  those  cases  where 
Ladar  scans  do  not  contain  enough  features  to  compute  a  laser-based  position.  In  those  cases  where  not 
enough  features  are  present  for  a  Ladar-based  position  solution,  the  navigator  of  Figure  18  will  employ  inertial 
coasting. 


The  dynamic-state  INS  calibration  uses  a  Kalman  filter  to  periodically  estimate  inertial  error  states.  The 
estimation  process  is  based  on  a  complementary  Kalman  filter  methodology  [21],  which  employs  differences 
between  INS  and  laser  scanner  observables  as  filter  measurements.  Changes  in  planar  surface  ranges  between 
consecutive  scans  are  used  as  laser  observables.  Correspondingly,  laser  scanner  observables  of  the  Kalman 
filter  are  formulated  as  follows  for  the  scan  at  time  epoch  tm: 


^Pls  (tm) 


Pi(tm-i)-pi(tm) 


.PltO-m-l)  PfcO-m). 


(20) 


where  K  is  the  number  of  features  for  which  the  match  is  found  between  time  epochs  tm  and  tm_i. 

Equivalent  observables  can  be  synthesized  from  INS  measurements  by  transformation  of  the  INS 
displacement  vector  into  the  range  domain  as  follows: 

ApINS(tm)  =  H(tm_1)(ARINS(tm)  +  (c^(tm)-C^(tm_1))b)  (21) 

where  H(tm-l)  is  the  observation  matrix  at  time  epoch  m-1  given  by  equation  (9),  ARINS(tm)  is  displacement 
from  time  epoch  m-1  to  m  calculated  from  the  IMU  measurements,  and  lb  is  the  lever  arm  vector  pointed  from 
the  IMU  to  the  Ladar  scanner  with  the  vector  components  given  at  the  laser  scanner-defined  body  frame.  As 
mentioned  previously,  filter  measurements  are  defined  as  differences  between  inertial  and  laser  scanner 
observables: 

y  Kalman  (^m  )  =  ^PlNS  (^m  )  ~~  ^PlS  (^m  )  (22) 


The  filter  operates  with  dynamic  states  only  (position  error  states  are  not  used).  Particular  filter  states  include: 
errors  in  position  changes  between  consecutive  scans,  velocity  errors,  attitude  errors,  gyro  biases,  and 
accelerometer  biases: 


5x  =  [5ARJ,  5v^  v|/t 


(23) 
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For  this  state  vector,  the  observation  matrix  HKaiman  can  be  derived  directly  by  augmenting  the  geometry 
matrix  of  equation  (16)  with  zero  elements: 


H 


Kalman  m 


(O 


HlcO-m-l)  0 


0 

0 


(24) 


The  measurement  noise  matrix  Reiman  is  derived  from  the  line  and  planar  surface  estimation  processes 
performing  a  comprehensive  covariance  analysis  of  the  feature  extraction  method  [27][28][31]. 

Derivation  of  the  filter  state  transition  matrix  and  the  system  noise  matrix  for  the  filter  states  chosen  employs 
a  standard  Kalman  filter  formulation,  which  can  be  found  in  [21].  Now,  the  complementary  Kalman  filter  can 
be  implemented  to  estimate  the  inertial  error  states  in  a  manner  similar  to  the  once  discussed  in  the  image- 
aided  navigation  section  using  the  filter  states,  the  filter  measurements,  and  the  filter  matrices.  Note  that  the 
Kalman  filter  implementation  considered  herein  does  not  take  into  account  a  correlation  between  errors  in 
estimated  ranges  of  extracted  line  normal  points  and  inertial  attitude  errors. 

It  is  important  to  mention  that  the  addition  of  new  lines  to  the  line  list  generally  improves  the  feature  geometry 
and  availability  but  at  the  same  time  creates  a  drift  in  the  relative  position  solution.  If  the  same  lines  are  seen 
in  the  initial  scan  and  the  following  scans  then  errors  in  the  relative  position  estimate  are  due  to  line  extraction 
errors  in  the  current  and  initial  scans  only.  For  this  case,  the  position  solution  does  not  drift.  If  a  new  line  is 
seen,  it  is  transformed  into  the  initial  frame  using  the  current  position  estimate.  Errors  in  the  position  estimate 
are  thus  transformed  into  errors  in  line  parameters  and  add  up  to  line  extraction  errors.  As  a  result,  the  current 
position  error  contributes  to  the  position  error  for  the  next  scan  where  the  new  line  is  used  for  navigation.  A 
position  drift  is  thus  created.  This  position  drift  is  however  generally  significantly  smaller  as  compared  to  the 
case  where  position  increments  are  estimated  based  on  consecutive  scans  thus  introducing  a  random  walk. 

Parameters  of  a  planar  surface  can  be  estimated  by  exploiting  motion  of  a  2D  Ladar  scanner  in  order  to 
observe  the  surface  from  different  points  of  view.  This  is  illustrated  in  Figure  20.  Now  the  multiple  observed 
lines  can  be  transformed  to  a  common  frame  using  the  INS  data  and  associated  to  form  lines  belonging  to  the 
same  planar  surface.  Multiple  of  these  lines  can  then  be  used  to  estimate  the  planar  surface  parameters.  More 
details  of  this  method  can  be  found  in  [3 1]. 
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Figure  20:  Estimating  planar  surface  parameters  using  2D  Ladar  scans  and  the  Ladar’s  motion. 


3.3  Performance  Demonstration 

To  show  the  performance  of  the  algorithms  discussed  in  Section  3.2  for  2D  and  3D  navigation,  three  sets  of 
results  are  shown.  In  all  three  cases  a  SICK  LMS-200  Ladar  scanner  and  a  Systron  Donner  DQI  tactical  grade 
IMU  were  used.  For  the  first  test  the  equipment  was  mounted  on  a  test  cart  that  was  manually  pushed  through 
hallways  of  the  Ohio  University  Stocker  Engineering  Center.  To  create  a  reference  trajectory,  marks  were  put 
on  the  floor  along  the  test  path  as  illustrated  by  Figure  21.  These  marks  were  used  as  reference  points.  The 
cart  was  stopped  at  each  reference  point  and  the  laser/inertial-integrated  readings  were  compared  with 
reference  point  locations.  The  true  trajectory  and  the  estimated  trajectories  are  shown  in  the  graph  in  Figure 
21.  The  relative  position  errors  of  the  indoor  test  were  found  to  be  limited  to  10  cm  after  traveling  a  distance 
of  9  m  (or  1.1%  of  distance  traveled).  A  cm-accurate  accurate  trajectory  reconstruction  is  thus  demonstrated 
for  the  indoor  test  scenario  considered. 
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Reconstructed  test  trajectory 


O  Reference  points 
O  LS/INS  trajectory 


Figure  21:  Indoor  hallway  test  environment  and  test  results. 


In  the  second  scenario,  the  test  setup  was  mounted  on  a  van  and  driven  through  some  of  the  more  challenging 
urban  environment  of  Athens,  Ohio.  Since  GPS  was  unavailable  on  much  of  the  trajectory,  a  good  truth 
reference  could  not  be  established.  Hence,  the  overall  performance  was  measured  by  investigating  the  residual 
errors  of  the  Kalman  filter  as  well  as  the  overall  drift  after  each  of  the  scenarios.  This  drift  error  was  estimated 
by  computing  the  van’s  beginning  and  end  position  using  kinematic  GPS  and  computing  the  difference 
between  the  end  position  from  the  survey  and  the  end-position  as  calculated  by  the  algorithm.  Figure  22 
shows  the  scenario  for  the  urban  environment  scenario  and  the  trajectory  as  estimated  by  the  algorithm.  The 
drift  error  over  the  whole  trajectory  was  determined  to  have  a  magnitude  equal  to  1.59  m. 


Figure  22:  Outdoor  urban  environment  scenario  and  van  trajectory. 
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Finally,  the  3D  navigation  scenario  was  evaluated  in  a  laboratory  environment  using  actual  test  equipment  as 
shown  in  Figure  23.  Position  errors  shown  are  at  the  cm-level.  Standard  deviations  of  the  errors  in  x,  y,  and  z 
position  components  are  computed  as  3.5  cm,  0.7  cm,  and  2.1  cm,  respectively.  As  shown  in  more  details  in 
[31],  orientation  measurements  were  made  as  well.  The  standard  deviation  of  the  errors  in  pitch,  roll,  and 
heading  angles  are  estimated  to  be  1.6  deg,  2.5  deg,  and  1  deg,  respectively.  It  is  noted  that  attitude  errors  for 
the  live  data  test  are  increased  notably  as  compared  to  attitude  errors  that  were  done  using  simulations  (from 
sub-degree  level  to  degree  level).  This  error  increase  is  mainly  attributed  to  the  deviation  between  the 
commanded  angular  value  and  the  actual  elevation  angle  of  the  low-cost  servomotor  used  in  the  test  setup. 
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Figure  23:  Test  environment,  planar  surfaces  and  the  reconstruction  of  3D  translational  motion: 
true  trajectory  vs.  motion  trajectory  estimated  by  the  3D  planar-based  navigation  algorithm. 


4.0  A  LOOK  INTO  THE  FUTURE 

Advances  in  navigation  sensor  and  integration  technologies  have  allowed  us  to  equip  a  wide  variety  of 
platforms  with  an  integrated  navigation  capability  that  has  not  been  cost  effective  in  the  past.  The  modem 
commander  demands  precise  location  information  of  operational  assets,  be  they  aircraft,  ships,  spacecraft, 
ground  vehicles  (both  manned  and  robotic)  as  well  as  individual  soldiers.  This  capability  will  enhance 
situational  awareness  and  command  and  control,  increase  speed  of  maneuver,  improve  weapon  effectiveness, 
and  simplify  communications  and  logistics  requirements. 

Navigation  systems  will  continue  to  get  progressively  smaller  and  more  affordable.  Technological  advances, 
improved  mass  production  techniques  and  further  cost  reductions  are  forecast  for  the  foreseeable  future  for 
both  inertial  sensors  and  satellite  systems.  Integrated  navigation  systems,  such  as  INS/GPS,  will  continue  to 
be  the  standard  and  applicable  to  almost  any  platform  or  application.  These  systems  will  provide  accurate 
position,  attitude,  velocity  and  extremely  accurate  time  information  to  the  user  continuously.  New  sensor 
technologies,  innovative  methods  for  exploiting  signals  of  opportunity,  bio-inspired  navigation  concepts,  and 
improved  data  integration  algorithms  are  all  actively  being  pursued  to  handle  the  difficult  case  of  navigating 
when  satellite  navigation  is  not  always  available.  This  will  be  particularly  useful  for  ground  forces  operating 
in  complex  urban,  indoor  and  subterranean  environments. 
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Some  of  the  new  and  emerging  military  applications  of  future  integrated  navigation  systems  will  include: 

•  Networking  of  sensor  platforms  to  create  new  and  highly  effective  integrated  network  sensor  systems. 

•  Robotic  assets  that  use  precise  navigation  systems  to  allow  autonomous  maneuver,  alone  or  in  concert 
with  others. 

•  Guided  munitions  (e.g.  artillery  shells)  that  can  attack  a  given  coordinate  precisely,  independent  of 
aiming  accuracy  will  soon  be  a  reality. 

•  Accurate  positioning  of  all  platforms  even  when  operating  in  GPS  denied  environments. 

•  Other  innovative  applications  that  we  have  not  yet  imagined. 


5.0  SUMMARY 

Navigating  in  indoor  and  highly  urban  locations  is  a  “navigation  gap”  where  GPS  cannot  currently  perform, 
and  the  use  of  Alt-Nav  techniques  is  one  potential  way  to  fill  that  navigation  gap.  The  concept  of  integrated 
navigation  is  to  combine  the  outputs  of  different  types  of  sensors  (see  Figure  24).  As  we  have  discussed, 
navigation  sensors,  like  any  system,  have  strengths  and  weaknesses.  For  example,  GPS  has  exceptional 
accuracy,  but  it  is  subject  to  outages  due  to  the  loss  of  satellite  signals.  Inertial  sensors  rely  only  on  gravity 
and  platform  dynamics,  which  cannot  be  jammed,  but  they  exhibit  errors  that  grow  over  time  and  eventually 
become  unacceptable.  Through  the  careful  design  of  the  integrated  system,  the  limitations  of  individual 
sensor  technologies  can  be  greatly  mitigated  and  improved  accuracy  and  robustness  can  be  achieved. 
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Figure  24:  Modular,  adaptive,  multi-sensor  integrated  systems  provide  a  possible  solution 
to  providing  robust  navigation  to  dismounted  soldiers  in  complex  terrain 
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DISCLAIMER 

The  views  expressed  in  this  article  are  those  of  the  authors  and  do  not  reflect  the  official  policy  or  position  of 
the  United  States  Air  Force,  Department  of  Defense,  or  the  U.S  Government. 
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